/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package edu.wpi.first.wpilibj.templates.subsystems;

import edu.wpi.first.wpilibj.CANJaguar;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.templates.Recoverable;
import edu.wpi.first.wpilibj.templates.commands.manualDrive;

/**
 *
 * @author matt
 *
 *
 */
public class DriveSubsystem extends Subsystem implements Recoverable {

    // HandleDriveCAN class gets the majority of the repeated drive CAN stuff
    //  out of the main thread (and reduce the likelyhood of Driverstation
    //  timeouts.
    // Also copies the master motor voltages to the slave motors
    private class ModeCommand {

        public boolean newCommand;
        public DriveMode mode;
        public final Object sync = this;

        public void set(ModeCommand command) {
            newCommand = command.newCommand;
            mode = command.mode;
        }

        public ModeCommand() {
            newCommand = false;
            mode = DriveMode.kUninitialized;
        }
    }
    private ModeCommand coordinatedModeCommand = new ModeCommand();

    private class SpeedCommand {

        public boolean newCommand;
        public double speed;
        public final Object sync = this;

        public void set(SpeedCommand command) {
            newCommand = command.newCommand;
            speed = command.speed;
        }

        public SpeedCommand() {
            newCommand = false;
            speed = 0;
        }
    }
    private SpeedCommand coordinatedSpeedCommand = new SpeedCommand();

    private class MoveCommand {

        public boolean newCommand;
        public double distance;
        public final Object sync = this;

        public void set(MoveCommand command) {
            newCommand = command.newCommand;
            distance = command.distance;
        }

        public MoveCommand() {
            newCommand = false;
            distance = 0;
        }
    }
    private MoveCommand coordinatedMoveCommand = new MoveCommand();

    private class CoastCommand {

        public boolean newCommand;
        public boolean coast;
        public final Object sync = this;

        public void set(CoastCommand command) {
            newCommand = command.newCommand;
            coast = command.coast;
        }

        public CoastCommand() {
            newCommand = false;
            coast = false;
        }
    }
    private CoastCommand coordinatedCoastCommand = new CoastCommand();

    private class CurrentData {

        public double position;
        public final Object sync = this;
    }
    private CurrentData coordinatedCurrentData = new CurrentData();

    private class HandleDriveCAN implements Runnable {

        // local working version of the speed command
        private SpeedCommand speedCommand = new SpeedCommand();
        private CoastCommand coastCommand = new CoastCommand();
        private MoveCommand moveCommand = new MoveCommand();
        private ModeCommand modeCommand = new ModeCommand();
        private final double kTimeBetweenCommands = 0.1;
        private double lastTime = Timer.getFPGATimestamp();

        public void run() {
            // Run forever
            while (true) {
                // Copy the master motor voltages to the slave motors
                doCopyMasterToSlaves();

                doGetCurrentPosition();

//                System.out.println(kTimeBetweenCommands + " < "+(Timer.getFPGATimestamp() - lastTime)  + " ("+ Timer.getFPGATimestamp() + " - " + lastTime + ")");
                if (kTimeBetweenCommands < (Timer.getFPGATimestamp() - lastTime)) {
                    lastTime = Timer.getFPGATimestamp();
//                    System.out.println("%%");

// Do the various CAN based commands below

                    // set the drive speed command values on the master
                    synchronized (coordinatedSpeedCommand.sync) {
                        // set the working  copy of the speed command to the one that was set in the command
                        speedCommand.set(coordinatedSpeedCommand);
                        coordinatedSpeedCommand.newCommand = false;
                    }

                    if (speedCommand.newCommand) {
                        doDrive(speedCommand.speed);
                    }


                    // set the drive move command values on the master
                    synchronized (coordinatedMoveCommand.sync) {
                        // set the working  copy of the speed command to the one that was set in the command
                        moveCommand.set(coordinatedMoveCommand);
                        coordinatedMoveCommand.newCommand = false;
                    }

                    if (moveCommand.newCommand) {
                        doDrive(moveCommand.distance);
                    }

                    // Set Jaguar Motor Coast Feature
                    synchronized (coordinatedCoastCommand.sync) {
                        // set the working  copy of the speed command to the one that was set in the command
                        coastCommand.set(coordinatedCoastCommand);
                        coordinatedCoastCommand.newCommand = false;
                    }

                    if (coastCommand.newCommand) {
                        doCoast(coastCommand.coast);
                    }

                    // set the drive mode
                    synchronized (coordinatedModeCommand.sync) {
                        modeCommand.set(coordinatedModeCommand);
                        modeCommand.newCommand = false;
                    }

                    if (modeCommand.newCommand) {
                        if (DriveMode.kDistanceControl == modeCommand.mode) {
                            doSetDistanceMode();
                        } else { // default mode is speed
                            doSetSpeedMode();
                        }
                    }
                }
            }
        }
    }

    private void doCopyMasterToSlaves() {
        for (int idx = m_kSlaveOneIdx; idx < m_maxMotors; idx++) {

            try {
                if (null != driveMotor[idx]) {
                    // Set the slave motor voltages to the master voltage
                    driveMotor[idx].setX(driveMotor[m_kMasterIdx].getOutputVoltage());
                }

            } catch (edu.wpi.first.wpilibj.can.CANTimeoutException e) {
                System.out.println("DriveSubsystem(" + sideName + ") " + Timer.getFPGATimestamp() + ": CAN Timeout while copying voltage to slave motor CAN ID " + m_motorID[idx]);
                // set this motor to null since it wouldn't create (that way other parts of the class can tell not to use it)
            } catch (Exception all) {
                System.out.println(all.toString());
            }

            try {
                CANJaguar.updateSyncGroup(m_syncGroup);
            } catch (edu.wpi.first.wpilibj.can.CANTimeoutException e) {
                System.out.println("DriveSubsystem.drive(" + sideName + ")" + Timer.getFPGATimestamp() + ": CAN Timeout while updating slave sync group " + m_syncGroup);
            }
        }

    }
    final double kTimeBetweenPositionUpdates = 0.1; // .1s (10Hz)
    private double lastTime = Timer.getFPGATimestamp();

    private void doGetCurrentPosition() {

        // if in distance mode
        if (DriveMode.kDistanceControl == m_driveMode) {
            if (kTimeBetweenPositionUpdates > (Timer.getFPGATimestamp() - lastTime)) {
                lastTime = Timer.getFPGATimestamp();
                double position;
                try {
                    position = driveMotor[m_kMasterIdx].getPosition();
                } catch (edu.wpi.first.wpilibj.can.CANTimeoutException e) {
                    System.out.println("DriveSubsystem.drive(" + sideName + "): CAN Timeout while reading distance traveled");
                    // if we didn't get data this time, use last times
                    synchronized (coordinatedCurrentData.sync) {
                        position = coordinatedCurrentData.position;
                    }
                }
                synchronized (coordinatedCurrentData.sync) {
                    coordinatedCurrentData.position = position;
                }
            }
        }
    }

    private void doDrive(double commandSpeed) {
        // Set speeds for the drivetrains

        if (DriveMode.kSpeedControl == m_driveMode) {
            if (driveMotor[m_kMasterIdx] != null) {  // only command valid motors
                try {
                    //System.out.println("&" + m_syncGroup + " - " + commandSpeed);
                    driveMotor[m_kMasterIdx].setX(m_kMaxMotorSpeed * commandSpeed, m_syncGroup);
                } catch (edu.wpi.first.wpilibj.can.CANTimeoutException e) {
                    System.out.println("DriveSubsystem.drive(" + sideName + ")" + Timer.getFPGATimestamp() + ": CAN Timeout while setting master motor speed on motor " + m_motorID[m_kMasterIdx]);
                }
            }
        } else if ((DriveMode.kUninitialized == m_driveMode) || (driveMotor[m_kMasterIdx] == null)) {
            for (int idx = 0; idx < m_maxMotors; idx++) {
                try {
                    if (driveMotor[idx] != null) {  // only command valid motors
                        driveMotor[idx].setX(m_kMaxMotorSpeed * commandSpeed, m_syncGroup);
                    }
                } catch (edu.wpi.first.wpilibj.can.CANTimeoutException e) {
                    System.out.println("DriveSubsystem.drive(" + sideName + ")" + Timer.getFPGATimestamp() + ": CAN Timeout while setting motor speed on motor " + m_motorID[idx]);
                }
            }
        }

        try {
            CANJaguar.updateSyncGroup(m_syncGroup);
        } catch (edu.wpi.first.wpilibj.can.CANTimeoutException e) {
            System.out.println("DriveSubsystem.drive(" + sideName + ")" + Timer.getFPGATimestamp() + ": CAN Timeout while updating sync group " + m_syncGroup);
        }

    }

    private void doSetSpeedMode() {
        // set the master to PID on the encoders (if it exists)
        if (null != driveMotor[m_kMasterIdx]) {
            double gain_Kp = 1.0; // Proportional
            double gain_Ki = 0.0; // Integral
            double gain_Kd = 0.0; // Derivative

            int tries = 0;
            boolean configured = false;
            do {
                try {
                    driveMotor[m_kMasterIdx].changeControlMode(CANJaguar.ControlMode.kSpeed);
                    driveMotor[m_kMasterIdx].setPositionReference(CANJaguar.PositionReference.kQuadEncoder);
                    driveMotor[m_kMasterIdx].configEncoderCodesPerRev(m_kEncoderCountsPerRevolution);
                    driveMotor[m_kMasterIdx].setPID(gain_Kp, gain_Ki, gain_Kd);
                    driveMotor[m_kMasterIdx].setX(0, m_syncGroup);
                    driveMotor[m_kMasterIdx].enableControl();
                    configured = true;
                } catch (edu.wpi.first.wpilibj.can.CANTimeoutException e) {
                    System.out.println("DriveSubsystem(" + sideName + "): CAN Timeout while configuring speed master CAN ID " + m_motorID[m_kMasterIdx]);
                } catch (Exception all) {
                    System.out.println(all.toString());
                }
                tries++;
            } while (!configured && (tries < m_kMaxRetries));
            if (configured) {
                m_driveMode = DriveMode.kSpeedControl;
                System.out.println("DriveSubsystem(" + sideName + "): Configured speed master motor " + m_motorID[m_kMasterIdx]);
            }
        }

    }

    private void doSetDistanceMode() {
        // set the master to PID on the encoders (if it exists)
        // and spin up the voltage drive copy teask for the other motors
        if (null != driveMotor[m_kMasterIdx]) {
            double gain_Kp = 1.0; // Proportional
            double gain_Ki = 0.0; // Integral
            double gain_Kd = 0.0; // Derivative

            int tries = 0;
            boolean configured = false;
            do {
                try {
                    driveMotor[m_kMasterIdx].changeControlMode(CANJaguar.ControlMode.kPosition);
                    driveMotor[m_kMasterIdx].setPositionReference(CANJaguar.PositionReference.kQuadEncoder);
                    driveMotor[m_kMasterIdx].configEncoderCodesPerRev(m_kEncoderCountsPerRevolution);
                    driveMotor[m_kMasterIdx].setPID(gain_Kp, gain_Ki, gain_Kd);
                    driveMotor[m_kMasterIdx].setX(0, m_syncGroup);
                    driveMotor[m_kMasterIdx].enableControl(0); // set current position as zero
                    configured = true;
                } catch (edu.wpi.first.wpilibj.can.CANTimeoutException e) {
                    System.out.println("DriveSubsystem(" + sideName + "): CAN Timeout while configuring distance master CAN ID " + m_motorID[m_kMasterIdx]);
                } catch (Exception all) {
                    System.out.println(all.toString());
                }
                tries++;
            } while (!configured && (tries < m_kMaxRetries));
            if (configured) {
                m_driveMode = DriveMode.kSpeedControl;
                System.out.println("DriveSubsystem(" + sideName + "): Configured distance master motor " + m_motorID[m_kMasterIdx]);
            }
        }
    }

    private void doMove(double distance) {

        commandRevolutions = distance / m_kMotorRevsPerMeter;

        if (DriveMode.kDistanceControl == m_driveMode) {
            if (driveMotor[m_kMasterIdx] != null) {  // only command valid motors
                try {
                    driveMotor[m_kMasterIdx].setX(commandRevolutions, m_syncGroup);
                } catch (edu.wpi.first.wpilibj.can.CANTimeoutException e) {
                    System.out.println("DriveSubsystem.drive(" + sideName + ")" + Timer.getFPGATimestamp() + ": CAN Timeout while setting master motor distance on motor " + m_motorID[m_kMasterIdx]);
                }
            }
        }

        try {
            CANJaguar.updateSyncGroup(m_syncGroup);
        } catch (edu.wpi.first.wpilibj.can.CANTimeoutException e) {
            System.out.println("DriveSubsystem.drive(" + sideName + ")" + Timer.getFPGATimestamp() + ": CAN Timeout while updating sync group " + m_syncGroup);
        }

    }

    private void doCoast(boolean coast) {

        for (int idx = 0; idx < m_maxMotors; idx++) {
            try {
                // initially just set use output voltage mode
                if (driveMotor[idx] != null) {  // only command valid motors
                    if (coast) {
                        driveMotor[idx].configNeutralMode(CANJaguar.NeutralMode.kCoast);
                    } else {
                        driveMotor[idx].configNeutralMode(CANJaguar.NeutralMode.kBrake);
                    }
                }
            } catch (edu.wpi.first.wpilibj.can.CANTimeoutException e) {
                System.out.println("DriveSubsystem.drive(" + sideName + "): CAN Timeout while setting coast/break on motor " + m_motorID[idx]);
            }
        }
    }

    // Drive Mode Enumeration
    public static class DriveMode {

        public final int value;
        // Drive Mode enumeration constants
        protected static final int m_kUninitialized = 0;
        protected static final int m_kSpeedControl = 1;
        protected static final int m_kDistanceControl = 2;
        public static final DriveMode kUninitialized = new DriveMode(m_kUninitialized);
        public static final DriveMode kSpeedControl = new DriveMode(m_kSpeedControl);
        public static final DriveMode kDistanceControl = new DriveMode(m_kDistanceControl);

        private DriveMode(int DriveMode) {
            this.value = DriveMode;
        }
    }
// Drive Mode
    private DriveMode m_driveMode = DriveMode.kUninitialized;
    // Name of the side
    private String sideName;
    // Speed rate multiplier
    private static final double m_kMaxMotorSpeed = 130;
    // Number of motor commandRevolutions per meter travelled
    private static final double m_kMotorRevsPerMeter = 50; // TBD
    // Put methods for controlling this subsystem
    // here. Call these from Commands.
    // supports up to 3 motors
    private static final int m_maxMotors = 3;
    // Try up to 5 times
    private static final int m_kMaxRetries = 5;
    // We're using a nominal 12V power supply
    private static final double m_kMaxVolts = 12;
    // Encoder counts per revolution
    int m_kEncoderCountsPerRevolution = 250;
    // instance count and sync group 
    // The instance count is used to generate a differnt sync group for each
    // instance of this class
    private static byte m_instanceCount = 0;
    private byte m_syncGroup = 1;
    // Note CANJaguar uses group 0 for it's own purposes so we pick other numbers
    // Motor Array Indices
    private final static int m_kMasterIdx = 0;
    private final static int m_kSlaveOneIdx = 1;
    private final static int m_kSlaveTwoIdx = 2;
    // Motor Array
    protected CANJaguar driveMotor[] = {null, null, null};
    // Motor CAN ID Arrays
    private int m_motorID[] = {1, 1, 1};
    // Whether on not the drive direction is reversed
    private boolean m_reverseDrive = false;
    // Current drivetrain position (only valid in distance drive mode (in revolutoions))
    private double currentPosition = 0;
    // Tolerance for isInPosition
    private final double m_kPositionTolerance = 0.1;
    double commandRevolutions;
    double commandSpeed;

    public DriveSubsystem(String side, final int master, final int slaveOne, final int slaveTwo) {
        // System.out.println("Constructing DriveSubsystem " + master + ", " + slaveOne + ", " + slaveTwo + ", ");
        sideName = side;

        m_motorID[m_kMasterIdx] = master;
        m_motorID[m_kSlaveOneIdx] = slaveOne;
        m_motorID[m_kSlaveTwoIdx] = slaveTwo;

        // Co mpute a new sync group for this instance
        //  The approach below will fall apart if we have more than 8 of this class
        //  but since we only will have 2 we're OK
        m_instanceCount++;
        m_syncGroup = (byte) (m_syncGroup << m_instanceCount);

        //System.out.println("Sync Group is " + m_syncGroup);

        // Create and initialize the motor objects
        // initislly set all motors to voltage control
        for (int idx = 0; idx < m_maxMotors; idx++) {
            int tries = 0;
            do {
                try {
                    driveMotor[idx] = new CANJaguar(m_motorID[idx]);
                } catch (edu.wpi.first.wpilibj.can.CANTimeoutException e) {
                    System.out.println("DriveSubsystem(" + sideName + "): CAN Timeout while creating motor interface for CAN ID " + m_motorID[idx]);
                    // set this motor to null since it wouldn't create (that way other parts of the class can tell not to use it)
                    driveMotor[idx] = null;
                } catch (Exception all) {
                    System.out.println(all.toString());
                }
                tries++;
            } while ((null == driveMotor[idx]) && tries < m_kMaxRetries);
            if ((null == driveMotor[idx])) {
                System.out.println("DriveSubsystem(" + sideName + "): created motor " + m_motorID[idx]);
            }
        }

        // Set the slave motors to voltage control
        for (int idx = m_kSlaveOneIdx; idx < m_maxMotors; idx++) {
            int tries = 0;
            boolean configured = false;
            do {
                try {
                    if (null != driveMotor[idx]) {
                        driveMotor[idx].changeControlMode(CANJaguar.ControlMode.kVoltage);
                        driveMotor[idx].enableControl();
                    }
                    configured = true;
                } catch (edu.wpi.first.wpilibj.can.CANTimeoutException e) {
                    System.out.println("DriveSubsystem(" + sideName + "): CAN Timeout while confguring slave motor CAN ID " + m_motorID[idx]);
                    // set this motor to null since it wouldn't create (that way other parts of the class can tell not to use it)
                } catch (Exception all) {
                    System.out.println(all.toString());
                }
                tries++;
                //System.out.println("Tries equals:" + tries);
            } while (!configured && (tries < m_kMaxRetries));
            if (configured) {
                System.out.println("DriveSubsystem(" + sideName + "): Configured slave motor " + m_motorID[idx]);
            }
        }

        // Start the CAN Communications thread
        (new Thread(new HandleDriveCAN())).start();
        System.out.println("DriveSubsystem(" + sideName + "): CAN Handler Thread Started ");

        // Start out in speed mode
        setSpeedMode();

        System.out.println(sideName + " DriveSubsystem constructed");
    }

    public void initDefaultCommand() {
        // Set the default command for a subsystem here.
        setDefaultCommand(new manualDrive());
    }

    public void drive(double speed) {

        // Set speeds for the drivetrain
        commandSpeed = speed;

        if (m_reverseDrive) {
            commandSpeed = -commandSpeed;
        }

        synchronized (coordinatedSpeedCommand.sync) {
            coordinatedSpeedCommand.speed = commandSpeed;
            coordinatedSpeedCommand.newCommand = true;
        }

    }

    public final void setDistanceMode() {
        synchronized (coordinatedModeCommand.sync) {
            coordinatedModeCommand.mode = DriveMode.kDistanceControl;
            coordinatedModeCommand.newCommand = true;
        }
    }

    public final void setSpeedMode() {
        synchronized (coordinatedModeCommand.sync) {
            coordinatedModeCommand.mode = DriveMode.kSpeedControl;
            coordinatedModeCommand.newCommand = true;
        }
    }

    public void move(double distance) {
        if (m_reverseDrive) {
            distance = -distance;
        }

        synchronized (coordinatedMoveCommand) {
            coordinatedMoveCommand.distance = distance;
            coordinatedMoveCommand.newCommand = true;
        }
    }

    public void setReverse(boolean doReverse) {
        m_reverseDrive = doReverse;
    }

    public void setCoast(boolean coast) {
        synchronized (coordinatedCoastCommand.sync) {
            coordinatedCoastCommand.coast = coast;
            coordinatedCoastCommand.newCommand = true;
        }
    }

    public DriveMode getDriveMode() {
        return m_driveMode;
    }

    public double getPosition() {
        double position;
        synchronized (coordinatedCurrentData.sync) {
            position = coordinatedCurrentData.position;
        }
        return position;
    }

    public boolean isMotionComplete() {
        if (DriveMode.kSpeedControl == m_driveMode) {
            return false;
        } else if (DriveMode.kDistanceControl == m_driveMode) {
            return (((getPosition() * (1 - m_kPositionTolerance)) <= commandRevolutions)
                    && ((getPosition() * (1 + m_kPositionTolerance)) >= commandRevolutions));
        } else // if unknown or bad drive mode consider to always be in position
        {
            return true;
        }
    }

    /*
     * Power drop recovery support
     */
    public void recover(boolean override) {
        boolean needsRecovery = false;

        // Recover the slave motors
        for (int idx = m_kSlaveOneIdx; idx < m_maxMotors; idx++) {
            needsRecovery = false;

            // figure out if the Jaguar has rebooted
            try {
                needsRecovery = driveMotor[idx].getPowerCycled();
            } catch (edu.wpi.first.wpilibj.can.CANTimeoutException e) {
                System.out.println("DriveSubsystem(" + sideName + "): CAN Timeout while checking for power loss " + m_motorID[idx]);
            }

            // and if it did, reinitialize
            if (needsRecovery || override) {
                System.out.println("DriveSubsystem(" + sideName + ") " + Timer.getFPGATimestamp() + " Recovering " + m_motorID[idx]);
                int tries = 0;
                boolean configured = false;
                do {
                    try {
                        if (null != driveMotor[idx]) {
                            driveMotor[idx].changeControlMode(CANJaguar.ControlMode.kVoltage);
                            driveMotor[idx].enableControl();
                        }
                        configured = true;
                    } catch (edu.wpi.first.wpilibj.can.CANTimeoutException e) {
                        System.out.println("DriveSubsystem(" + sideName + "): CAN Timeout while reconfguring slave motor CAN ID " + m_motorID[idx]);
                        // set this motor to null since it wouldn't create (that way other parts of the class can tell not to use it)
                    } catch (Exception all) {
                        System.out.println(all.toString());
                    }
                    tries++;
                    //System.out.println("Tries equals:" + tries);
                } while (!configured && (tries < m_kMaxRetries));
                if (configured) {
                    System.out.println("DriveSubsystem(" + sideName + "): Reconfigured slave motor " + m_motorID[idx]);
                }
            }
        }

        // Recover the master motor
        needsRecovery = false;

        // figure out if the Jaguar has rebooted
        try {
            needsRecovery = driveMotor[m_kMasterIdx].getPowerCycled();
        } catch (edu.wpi.first.wpilibj.can.CANTimeoutException e) {
            System.out.println("DriveSubsystem(" + sideName + "): CAN Timeout while checking for power loss " + m_motorID[m_kMasterIdx]);
        }

        // and if it did, reinitialize
        if (needsRecovery || override) {
            System.out.println("DriveSubsystem(" + sideName + ") " + Timer.getFPGATimestamp() + " Recovering " + m_motorID[m_kMasterIdx]);
            if (DriveMode.kDistanceControl == m_driveMode) {
                setDistanceMode();
            } else {
                setSpeedMode();
            }
        }
    }
}
